/** Include Headder Files **/
#include "algorism.h"
#include "math.h"

/**
 * \brief 平滑滤波器初始化
 * \param[in] alpha 滤波系数
 */
void SmoothFilterInitialize(SmoothFilter* filter, float alpha)
{
    filter->alpha = alpha;
		filter->current = filter->last = 0;
}

/**
 * \brief 平滑滤波器更新 
 */
float SmoothFilterUpdate(SmoothFilter* filter, float input)
{
    filter->last = filter->current;
    return (filter->current = filter->alpha * filter->last + \
                              (1 - filter->alpha) * input);
}

/// @brief 滑窗滤波计算更新
/// @param filter 
/// @param input 当前原始数据输入
/// @return 滤波加权后的值
float SlidingWindowFilterFilterUpdate(SlidingWindowFilter* filter, float input)
{
	float current_output = 0;
	//calc last weighted value
	for(int i = filter->last_data_num - 1; i >= 0; i--)
		current_output += filter->last_data[i] * filter->k[i + 1];

	//add current weighted value
	current_output += filter->k[0] * input;

	//shift value
	for(int i = filter->last_data_num - 1; i > 0; i--)
		filter->last_data[i] = filter->last_data[i-1];
	filter->last_data[0] = current_output;
	
	return filter->last_data[0];
}

/**
 * \brief 角度限制
 * \param[in] angle 输入角度
 * \param[in] limitMIN 角度上限
 * \param[in] limitMAX 角度下限
 * \return 限幅后的角度值 
 */
float AngleLimit(float angle, float limit_min, float limit_max)
{
	if(limit_max == limit_min) 
		return angle;
	float stride = limit_max - limit_min;
	while  (angle < limit_min)
		angle += stride;
	while (angle > limit_max)
		angle -= stride;
	
	return angle;
}

/**
 * @brief 正负最大值限制
 */
float AbsLimiter(float val, float max)
{
	if(val > max) 
	{
		val = max;
		return val;
	}
	if(val < -max)
		val = -max;
	
	return val;
}

 
/**
 * @brief 双边限幅
 */float DoubleEdgeLimiter(float val, float min, float max)
{
	return (val > max ? max : (val < min ? min : val));
}

/**
  * @brief  fast inverse square-root, to calculate 1/Sqrt(x)
  * @param  x: the number need to be calculated
  * @retval 1/Sqrt(x)
  * @usage  call in OnboardIMUahrsUpdate() function
  */
float InvSqrt(float x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long *)&y;

	i = 0x5f3759df - (i >> 1);
	y = *(float *)&i;
	y = y * (1.5f - (halfx * y * y));

	return y;
}

/**
 * @brief 符号函数
 */
int Sign(float x){
	if (x>1e-9)
		return 1;
	else if(x<-1e-9)
		return -1;
	else
		return 0;
}

